#include "commsys_mgr.hpp"
#include "uartcomm.hpp"
#include "lincomm.hpp"
#include <iostream>
#include <memory>
#include <chrono>
#include "spdlog/spdlog.h"
#include "cancomm.hpp"
#include "tcpcomm.hpp"
// data received callback
void onDataReceived(const std::string& name, const std::vector<uint8_t>& data) {
    std::string hex_str;
    for (auto byte : data) {
        hex_str += fmt::format("{:02X} ", byte);
    }
    if (!hex_str.empty()) {
        hex_str.pop_back();
    }
    spdlog::info("[{}] Received data: {}", name, hex_str);
}

int main() {
    spdlog::set_level(spdlog::level::debug);

    auto& commSystemMgr = CommSystemManager::getInstance();
    
    // auto uart1 = std::make_shared<UartComm>("/dev/ttyS3", 4000000);
    // uart1->registerCallback([&uart1](const std::vector<uint8_t>& data) {onDataReceived(uart1->getName(), data);});
    // commSystemMgr.addInterface("uart1", uart1);
    
    auto lin = std::make_shared<LinComm>("/dev/ttyS3", 19200);
    lin->registerCallback([&lin](const std::vector<uint8_t>& data) {onDataReceived(lin->getName(), data);});
    commSystemMgr.addInterface("lin1", lin);

    // auto can = std::make_shared<CanComm>("can0",500000);
    // can->registerCallback([&can](const std::vector<uint8_t>& data) {onDataReceived(can->getName(), data);});
    // commSystemMgr.addInterface("can0", can);

    // auto tcp0 = std::make_shared<TcpComm>(8080);
    // tcp0->registerCallback([&tcp0](const std::vector<uint8_t>& data) {onDataReceived(tcp0->getName(), data);});
    // commSystemMgr.addInterface("tcp0", tcp0);

    // auto tcp1 = std::make_shared<TcpComm>("192.168.0.105",8080);
    // tcp1->registerCallback([&tcp1](const std::vector<uint8_t>& data) {onDataReceived(tcp1->getName(), data);});
    // commSystemMgr.addInterface("tcp1", tcp1);
    
    if (!commSystemMgr.initializeAll()) {
        spdlog::error("Failed to initialize all interfaces");
        return 1;
    }
    
    commSystemMgr.startMonitoring();
    
    try {
        while (true) {
            static int counter = 0;
            counter++;
            //std::vector<uint8_t> data = {0x01,0x02,0x03,0x04,0x05,0x06,0x07,static_cast<uint8_t>(counter++)};
            std::vector<uint8_t> LINdata08 = {0x08,0xFF,0xA3,0x00,0x10,0x00,0x00,0xF8,0x6f};
            std::vector<uint8_t> LINdata09 = {0x49,0x00,0xff,0x8f,0x34,0x9c,0x34,0x00,0x00};
            std::vector<uint8_t> LINdata10 = {0x50,0xca,0x8a,0x01,0xf0,0x00,0x02,0x30,0x00};
            std::vector<uint8_t> LINdata11 = {0x11,0x00,0xf0,0xfe,0xff,0x00,0x80,0x00,0xff};
            std::vector<uint8_t> LINdata12 = {0x92,0xff,0xff,0x8c,0x00,0x80,0xf8,0x01,0xff};

            if(counter >= 20)
            {
                counter = 0;
                
            }
            else if(counter >= 10)
            {
                LINdata10[2] = 0x8a;
                LINdata10[5] = 0x08;
            }
            else
            {
                LINdata10[2] = 0x0a;
                LINdata10[5] = 0x00;
            }

            // if (auto uart1 = commSystemMgr.getInterface("uart1")) {
            //     uart1->send(data);
            // }

            if (auto lin = commSystemMgr.getInterface("lin1")) {
                lin->send(LINdata08);
                lin->send(LINdata09);
                lin->send(LINdata10);
                lin->send(LINdata11);
                lin->send(LINdata12);
                
            }
            
            // if (auto can0 = commSystemMgr.getInterface("can0")) {
            //     can0->send(data);
            // }

            // if (auto tcp0 = commSystemMgr.getInterface("tcp0")) {
            //     tcp0->send(data);
            // }

            std::this_thread::sleep_for(std::chrono::milliseconds(70));
        }
    } catch (...) {
        commSystemMgr.stopMonitoring();
        commSystemMgr.shutdownAll();
        throw;
    }
    
    return 0;
}